#include "includes.h"
#include "delay.h"
#include "can1.h"
#include "imu.h"
#include "Judge_System.h"
#include "uart1.h"
#include "uart2.h"
#include "lib_math.h"
#include "task.h"
#include "user.h"
#include "adc.h"
#include "dma.h"

#define MID_VALUE 1024
#define DIV_ROCKER 2
#define vmax 7000
#define vmaxx 6000

//                                kp   ki  kd        error[3]  maxErr outPut maxOut
PID_ABS_t pid_chasis_201_velo = {0.0f, 0.0f, 0.0f, {0.0f, 0.0f, 0.0f}, 0.0f, 0.0f, 0.0f};
PID_ABS_t pid_Lfriction_202_velo = {0.0f, 0.0f, 0.0f, {0.0f, 0.0f, 0.0f}, 0.0f, 0.0f, 0.0f};
PID_ABS_t pid_Rfriction_203_velo = {0.0f, 0.0f, 0.0f, {0.0f, 0.0f, 0.0f}, 0.0f, 0.0f, 0.0f};

extern OS_TMR tmr1;
extern float this_201_angle;
extern float start_pos1;
extern float real_power;
extern uint16_t remain_power;
extern u8 reset_flag;
extern u8 start_fire_flag;
u8 friction_flag = 0;
int16_t vx;
int16_t target_velo_201 = 0;
int16_t target_velo_202 = 0;
int16_t target_velo_203 = 0;

ext_student_interactive_header_data_t send_data;

float look_s[3] = {0};
int debug_target = 4000;

void GUN_ON(void);
void GUN_OFF(void);

int rrrrrr = 0;
void Chasis_task(void *p_arg)
{
	OS_ERR err;
	p_arg = p_arg;

	/*******************************pid������ʼ��*******************************/
	//                             kp     ki     kd    max_error max_output
	PID_init(&pid_chasis_201_velo, 5.5f, 0.01f, 0.0f, 10000.0f, 4000.0f);
	PID_init(&pid_Lfriction_202_velo, 15.0f, 2.0f, 0.0f, 10000.0f, 8000.0f);
	PID_init(&pid_Rfriction_203_velo, 15.0f, 2.0f, 0.0f, 10000.0f, 8000.0f);

	/*********************************���Ա���***********************************/
	int shoot_fuck = 0;

/*********************************����ģʽ***********************************/
#ifdef chasis_debug_mode
	while (1)
	{
		target_velo_201 = 0; //(TelCtrlData.left_y - MID_VALUE) / 100;

		if (TeleCtrl_Rev_OK < 5)
		{

			if (TelCtrlData.switch_l == switch_middle || TelCtrlData.switch_l == switch_down)
			{
				target_velo_202 += 20;
				target_velo_203 -= 20;

				if (target_velo_202 >= debug_target)
				{
					target_velo_202 = debug_target;
					target_velo_203 = -debug_target;
				}
			}
			else
			{
				target_velo_202 -= 20;
				target_velo_203 += 20;
				if (target_velo_202 <= 0)
				{
					target_velo_202 = 0;
					target_velo_203 = 0;
				}
			}
			if (TelCtrlData.switch_r == switch_up)
			{
				vx = (TelCtrlData.left_y - MID_VALUE) >> DIV_ROCKER;
				target_velo_201 = 35 * vx;
			}

			look_s[0] = debug_target;
			look_s[1] = velocity_202;
			look_s[2] = -velocity_203;

			PID_Calc(&pid_Lfriction_202_velo, velocity_202, target_velo_202);
			PID_Calc(&pid_Rfriction_203_velo, velocity_203, target_velo_203);
			PID_Calc(&pid_chasis_201_velo, velocity_201, target_velo_201);
		}
		else
		{
			pid_Lfriction_202_velo.Output = pid_Rfriction_203_velo.Output = pid_chasis_201_velo.Output = 0;
		}
		Chasis_ESC_Send(pid_chasis_201_velo.Output, pid_Lfriction_202_velo.Output, pid_Rfriction_203_velo.Output);

		OSTimeDly(10, OS_OPT_TIME_PERIODIC, &err); //��ʱ10ms
	}
#endif
/*********************************����ģʽ***********************************/
#ifdef chasis_game_mode
	while (1)
	{
#ifdef up_gimbal
		if (++TeleCtrl_Rev_OK < 5)
#endif
#ifdef down_gimbal
			if (start_fire_flag == 4)
#endif
			{
#ifdef up_gimbal
				switch (TelCtrlData.switch_l)
				{
				case switch_up:
					GUN_OFF();
					break;
				case switch_middle:
					GUN_ON();
					break;
				case switch_down:
					GUN_ON();
					break;
				}

				if (TelCtrlData.switch_r == switch_up)
				{
					//reset_flag = 0;
					vx = (TelCtrlData.left_y - MID_VALUE) >> DIV_ROCKER;
					target_velo_201 = 35 * vx;

					PID_Calc(&pid_chasis_201_velo, velocity_201, target_velo_201);
				}
#endif
#ifdef down_gimbal
				GUN_ON();
#endif
			}
			else
			{
				pid_Lfriction_202_velo.Output = pid_Rfriction_203_velo.Output = pid_chasis_201_velo.Output = 0;
			}
		Chasis_ESC_Send(pid_chasis_201_velo.Output * sqrt(ext_power_heat_data_t.chassis_power_buffer / 200.f), pid_Lfriction_202_velo.Output, pid_Rfriction_203_velo.Output);
		OSTimeDly(10, OS_OPT_TIME_PERIODIC, &err); //��ʱ10ms
	}
#endif
}

void GUN_ON()
{
	target_velo_202 += 30;
	target_velo_203 -= 30;

	if (target_velo_202 > vmax)
	{
		target_velo_202 = vmax;	 //6000
		target_velo_203 = -vmax; //-6000
	}
	if (target_velo_202 > vmaxx && target_velo_203 < -vmaxx)
		friction_flag = 1;
	else
		friction_flag = 0;

	PID_Calc(&pid_Lfriction_202_velo, velocity_202, target_velo_202);
	PID_Calc(&pid_Rfriction_203_velo, velocity_203, target_velo_203);
}

void GUN_OFF()
{
	target_velo_202 -= 30;
	target_velo_203 += 30;
	if (target_velo_202 <= 0)
	{
		target_velo_202 = 0;
		target_velo_203 = 0;
	}
	PID_Calc(&pid_Lfriction_202_velo, velocity_202, target_velo_202);
	PID_Calc(&pid_Rfriction_203_velo, velocity_203, target_velo_203);
}
